#include "logging.h"
#include "SDHCFileSystem.h"

extern Serial pc;

SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
FILE *fp;


void clearState( SystemState *s ) {
    s->millis = 0;
    s->current = s->voltage = 0.0;
    s->gx = s->gy = s->gz = 0;
    s->gTemp = 0.0;
    s->ax = s->ay = s->az = 0;
    s->mx = s->my = s->mz = 0;
    s->gHeading = s->cHeading = 0.0;
    s->roll = s->pitch = s->yaw =0.0;
    s->gpsLatitude = s->gpsLongitude = s->gpsCourse = s->gpsHDOP = 0.0;
    s->lrEncDistance = s->rrEncDistance = 0.0;
    s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
    s->estHeading = s->estLatitude = s->estLongitude = s->estNorthing = s->estEasting = s->estX = s->estY = 0.0;
    s->nextWaypoint = 0;
    s->bearing = s->distance = 0.0;
}

//void logData( SystemState s, void (*logString)(char *s) ) {
void logData( SystemState s ) {
    char buf[256];

    sprintf(buf, "%d,%.1f,%.1f,%d,%d,%d,%.1f,%d,%d,%d,%d,%d,%d,%.1f,%.1f,%.1f,%.1f,%.1f,%.9f,%.9f,%.1f,%.1f,%.3f,%.3f,%.1f,%.1f,%.1f,%.1f,%.9f,%.9f,%.3f,%.3f,%.3f,%.3f,%d,%.1f,%.3f\n",
        s.millis,
        s.current, s.voltage,
        s.gx, s.gy, s.gz,
        s.gTemp,
        s.ax, s.ay, s.az,
        s.mx, s.my, s.mz,
        s.gHeading, s.cHeading,
        s.roll, s.pitch, s.yaw,
        s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsHDOP,
        s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading,
        s.estHeading, s.estLatitude, s.estLongitude, s.estNorthing, s.estEasting, s.estX, s.estY,
        s.nextWaypoint, s.bearing, s.distance
    );

    fprintf(fp, "%s", buf);

    return;    
}

// Find the next unused filename of the form logger##.csv where # is 0-9
//
bool initLogfile() 
{    
    char myname[64];
    bool status = false;
    
    pc.printf("Opening log file...\n");

    while (fp == 0) {
        if ((fp = fopen("/log/test.txt", "w")) == 0) {
            pc.printf("Waiting for filesystem to come online...");
            wait(0.200);
        }
    }    
    fclose(fp);

    for (int i = 0; i < 1000; i++) {
        sprintf(myname, "/log/log%04d.csv", i);
        //pc.printf("Try file: <%s>\n", myname);    
        if ((fp = fopen(myname, "r")) == 0) {
            //pc.printf("File not found: <%s>\n", myname);
            break;
        } else {
            //pc.printf("File exists: <%s>\n", myname);
            fclose(fp);
        }
    }    
    
    fp = fopen(myname, "w");
    if (fp == 0) {
        pc.printf("file write failed: %s\n", myname);
    } else {
        //status = true;
        pc.printf("opened %s for writing\n", myname);
        status = true;
        fprintf(fp, "s.millis, s.current, s.voltage, s.gx, s.gy, s.gz, s.gTemp, s.ax, s.ay, s.az, s.mx, s.my, s.mz, s.gHeading, s.cHeading, s.roll, s.pitch, s.yaw, s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsHDOP, s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading, s.estHeading, s.estLatitude, s.estLongitude, s.estNorthing, s.estEasting, s.estX, s.estY, s.nextWaypoint, s.bearing, s.distance\n");
        //fprintf(fp, "Millis, Gyro, GyroHdg, CompassHeading, Latitude, Longitude, Age, Date, Time, Altitude, Course, Speed, HDOP, ");
        //fprintf(fp, "Bearing, Distance, Left Enc, Right Enc, Gyro Temp, OdoHdg, Ax, Ay, Az, Mx, My, Mz\n");
    }
    
    return status;
}

void closeLogfile(void)
{
    if (fp) fclose(fp);
}